#ifndef CUBICSPLINE_HPP_
#define CUBICSPLINE_HPP_

#include "SplineBase.hpp"
#include "/usr/include/eigen3/Eigen/Dense"
#include <eigen3/Eigen/src/Core/Matrix.h>
#include <stdexcept>
#include <vector>
#include <cmath>

class CubicSpline:public SplineBase{
public:
  typedef Eigen::MatrixXd mat;
  typedef Eigen::VectorXd vec;
public:
  CubicSpline(const CubicSpline &) = default;
  CubicSpline(CubicSpline &&) = default;
  CubicSpline &operator=(const CubicSpline &) = default;
  CubicSpline &operator=(CubicSpline &&) = default;

  CubicSpline(vector_type x, vector_type y, bd_type type=natural, double l_val = 0, double r_val = 0):SplineBase(x,y){
    if (type == periodic) {
      if (std::abs(yData.front()-yData.back())>1e-3) {
        throw std::invalid_argument("invalid input for periodic spline");
      }
    }
    l_bd = l_val;
    r_bd = r_val;
    boudary = type;
    setupSplines();
  }

  CubicSpline(){}
  ~CubicSpline(){}
private:
  bd_type boudary;
  vector_type a, b, c; // a(x-xi)^3+b(x-xi)^2+c(x-xi)+yi;
  double l_bd = 0, r_bd = 0;   
public:
  // 
  void setData(vector_type x, vector_type y);

  //
  void setBoundary(bd_type bd, double l_val=0, double r_val=0);

  // compute the coefficient
  void setupSplines();

  // compute interpolate value
  double interpolate(double x) const;

  // compute derivatice
  double derivative(double x) const;

  // compute second derivative
  double secondDerivative(double x) const;
  
  // draw the picture
  void draw(std::string file_name) const{
    SplineBase::draw(file_name);
  };

  void check();
};

inline void CubicSpline::setData(vector_type x, vector_type y){
  xData = x;
  yData = y;
}

inline void CubicSpline::setBoundary(bd_type bd, double l_val, double r_val){
  if (bd == periodic) {
    if (std::abs(yData.front()-yData.back())>1e-3) {
      throw std::invalid_argument("invalid input for periodic spline");
    }
  }
  boudary = bd;
  l_bd = l_val;
  r_bd = r_val;
}

inline void CubicSpline::setupSplines() {
  int n = xData.size();
  // compute coefficient for three types of cubic spline
  if (boudary == complete) {
    // construct m to get the equation
    mat m = mat::Zero(n-2,n-2);
    vec v = vec::Zero(n-2);
    std::vector<double> h(n-1), delta(n-1);
    h[0] = xData[1] - xData[0], h[1] = xData[2] - xData[1];
    delta[0] = (yData[1] - yData[0]) / h[0], delta[1] = (yData[2] - yData[1]) / h[1];
    v(0) = 3 * (h[1] * delta[0] + h[0] * delta[1]) - h[1] * l_bd;
    m(0, 0) = 2 * (h[0] + h[1]), m(0, 1) = h[0];
    for (size_t i = 1; i < n - 3; i++) {
      h[i+1] = xData[i + 2] - xData[i + 1];
      delta[i+1] = (yData[i + 2] - yData[i+1]) / h[i+1];
      m(i, i - 1) = h[i+1];
      m(i, i) = 2 * (h[i] + h[i+1]);
      m(i, i + 1) = h[i];
      v(i) = 3 * (h[i] * delta[i+1] + h[i+1] * delta[i]);
    }
    h[n-2] = xData[n - 1] - xData[n - 2];
    delta[n-2] = (yData[n - 1] - yData[n - 2]) / h[n-2];
    m(n - 3, n - 4) = h[n-2];
    m(n - 3, n - 3) = 2 * (h[n-3] + h[n-2]);
    v(n - 3) = 3 * (h[n-3] * delta[n-2] + h[n-2] * delta[n-3]) - h[n-3] * r_bd;

    c.resize(n);
    b.resize(n-1);
    a.resize(n-1);
    c[0] = l_bd;

    // solving m d = v
    
    Eigen::FullPivLU<Eigen::MatrixXd> lu;
    lu.compute(m);
    Eigen::VectorXd d = lu.solve(v);

    for (size_t i = 1; i < n-1; i++) {
      c[i] = d(i-1);
    }
    c[n-1] = r_bd;
    for (size_t i = 0; i < n-1; i++){
      b[i] = (3*delta[i] - 2*c[i] - c[i+1])/h[i];
      a[i] = (c[i] - 2*delta[i] + c[i+1])/h[i]/h[i];
    }

  } else if (boudary == natural) {
    mat m = mat::Zero(n,n);
    vec v = vec::Zero(n);
    std::vector<double> h(n-1);
    std::vector<double> delta(n-1);
    h[0] = xData[1] - xData[0];
    delta[0] = (yData[1] - yData[0]) / h[0];
    v(0) = 3 * delta[0];
    m(0, 0) = 2, m(0, 1) = 1;
    for (size_t i = 1; i < n -1; i++) {
      h[i] = xData[i + 1] - xData[i];
      delta[i] = (yData[i + 1] - yData[i]) / h[i];
      m(i, i - 1) = h[i];
      m(i, i) = 2 * (h[i] + h[i-1]);
      m(i, i + 1) = h[i-1];
      v(i) = 3 * (h[i] * delta[i-1] + h[i-1] * delta[i]);
    }
    m(n-1, n-2) = 1;
    m(n-1, n-1) = 2;
    v(n - 1) = 3 * delta[n-2];

    c.resize(n);
    b.resize(n-1);
    a.resize(n-1);
    
    Eigen::FullPivLU<Eigen::MatrixXd> lu;
    lu.compute(m);
    Eigen::VectorXd d = lu.solve(v);

    for (size_t i = 0; i < n; i++) {
      c[i] = d(i);
    }
    for (size_t i = 0; i < n-1; i++){
      b[i] = (3*delta[i] - 2*c[i] - c[i+1])/h[i];
      a[i] = (c[i] - 2*delta[i] + c[i+1])/h[i]/h[i];
    }
  } else {
    mat m = mat::Zero(n-1,n-1);
    vec v = vec::Zero(n-1);
    std::vector<double> h(n-1), delta(n-1);
    h[0] = xData[1] - xData[0], h[1] = xData[2] - xData[1];
    delta[0] = (yData[1] - yData[0]) / h[0], delta[1] = (yData[2] - yData[1]) / h[1];
    v(0) = 3 * (h[1]*delta[0] + h[0]*delta[1]);
    m(0, 0) = 2*(h[0] + h[1]), m(0, 1) = h[0], m(0,n-2) = h[1];
    for (size_t i = 1; i < n - 2; i++) {
      h[i+1] = xData[i + 2] - xData[i + 1];
      delta[i+1] = (yData[i + 2] - yData[i + 1]) / h[i+1];
      m(i, i - 1) = h[i+1];
      m(i, i) = 2 * (h[i] + h[i+1]);
      m(i, i + 1) = h[i];
      v(i) = 3 * (h[i] * delta[i+1] + h[i+1] * delta[i]);
    }
    m(n-2, n-2) = 2*(h[0] + h[n-2]);
    m(n-2, n-3) = h[0];
    m(n-2, 0) = h[n-2];
    v(n - 2) = 3 * (h[0]*delta[n-2] + h[n-2]*delta[0]);

    c.resize(n);
    b.resize(n-1);
    a.resize(n-1);
    
    Eigen::FullPivLU<Eigen::MatrixXd> lu;
    lu.compute(m);
    Eigen::VectorXd d = lu.solve(v);

    for (size_t i = 1; i < n; i++) {
      c[i] = d(i-1);
    }

    c[0] = c[n-1];
    for (size_t i = 0; i < n-1; i++){
      b[i] = (3*delta[i] - 2*c[i] - c[i+1])/h[i];
      a[i] = (c[i] - 2*delta[i] + c[i+1])/h[i]/h[i];
    }
  }
}

inline double CubicSpline::interpolate(double x) const{
  size_t idx = get_closest_left_idx(x);
  double tmp = a[idx]*(x-xData[idx]);
  tmp = (tmp+b[idx])*(x-xData[idx]);
  tmp = (tmp+c[idx])*(x-xData[idx])+yData[idx];
  return tmp;  
}

inline double CubicSpline::derivative(double x) const{
  size_t idx = get_closest_left_idx(x);
  return (3*a[idx]*(x-xData[idx])+2*b[idx])*(x-xData[idx])+c[idx];
}

inline double CubicSpline::secondDerivative(double x) const{
  size_t idx = get_closest_left_idx(x);
  return 6*a[idx]*(x-xData[idx])+2*b[idx];
}

inline void CubicSpline::check(){
  if (boudary == periodic) {
    if (yData.front()!=yData.back()) {
      throw std::invalid_argument("invalid data");
    }
  }
}

#endif